#include "omni.h"

omni::omni( int whichOne ) : sensors( whichOne )
{
  msgEnviada = false;
}

omni::~omni()
{
  
}

int omni::go( int dir, float distance )
{
int n;
 
  timeOut = (20000*distance) + 2000000; // d=10cm -> timeOut=4s  
 
  if( dir == FRENTE )
     n = writeCmd(distance,OMNI_FWD,ARDUINO_OMNI);
       
  if( dir == TRAS )
     n = writeCmd(distance,OMNI_REV,ARDUINO_OMNI);
     
  if( dir == DIR )
     n = writeCmd(distance,OMNI_DIR,ARDUINO_OMNI);
  
  if( dir == ESQ )
     n = writeCmd(distance,OMNI_ESQ,ARDUINO_OMNI);

return n;     
}

int omni::turn( int sentido, float angles )
{
int n;
  
  timeOut = 40000*angles + 2000000; // 100graus -> timeOut=4s
  
  if( sentido == CW )
     n = writeCmd(angles,OMNI_CW,ARDUINO_OMNI);
  
  if( sentido == CCW )
     n = writeCmd(angles,OMNI_CCW,ARDUINO_OMNI);
  
return n;  
}


int omni::alinhaIR( int faceDoRobo, int intOUext )
{
  int n=0,instrucao = 0;
  char res[8];
  res[0] = '\0';
  
  timeOut = 10000000; // timeOut fixo de 10s
  
  instrucao = faceDoRobo*10 + intOUext; // msg enviada = <0(faceDoRobo)(intOUext)(identificador)>
  
  n = writeCmd(instrucao,OMNI_ALINHA_IR,ARDUINO_OMNI);

return n;  
}


struct timeval omni::le_t_envio()
{
  return t_envio;
}


bool omni::le_msgEnviada()
{
  return msgEnviada;
}

void omni::set_timeOutEstourado(bool arg)
{
  timeOutEstourado = arg;
}

void omni::reset( )
{
  char res[8];
  
  msgEnviada = true;
  
  while( (strcmp(res,"ACK")!=0) )
  {
      if( serialport_write(fdOmni,"[START]") == -1)
      { 
	cout << "Erro em: serialport_write( [START] )"<<endl;
	usleep(20000);
	continue; 
      }
      else
	cout << "msg enviada: " << "[START]" << endl;
      
      serialport_read_until(fdOmni,res,'\n');

  }
  msgEnviada = false;
  
  
}

long int omni::le_timeOut()
{
  return timeOut;
}

